#include "scope_core1.h"
#include "hardware/claim.h"

enum {
    SCOPE_CORE1_CMD_IDLE,
    SCOPE_CORE1_CMD_RUNNING,
    SCOPE_CORE1_CMD_FORCE_STOP,
} scope_core1_cmd_t;

static volatile int g_scope_state = SCOPE_CORE1_CMD_IDLE;

int __no_inline_not_in_flash_func(scope_core1_start)(void (*callback)(void))
{
    uint32_t save;
    
    save = hw_claim_lock();
    if (g_scope_state != SCOPE_CORE1_CMD_IDLE) {
        g_scope_state = SCOPE_CORE1_CMD_FORCE_STOP;
        hw_claim_unlock(save);
        // log_printf("force stop start\n");
        uint32_t start = time_us_32();
        while (time_us_32() - start < 100000) {
            if (g_scope_state == SCOPE_CORE1_CMD_IDLE) {
                break;
            }
        }
    } else {
        hw_claim_unlock(save);
    }
    if (g_scope_state != SCOPE_CORE1_CMD_IDLE) {
        // log_printf("core1 busy\n");
        return -1;
    }
    g_scope_state = SCOPE_CORE1_CMD_RUNNING;
    multicore_reset_core1();
    multicore_launch_core1(callback);
    return 0;
}

void scope_core1_stop(void)
{
    uint32_t save;
    
    save = hw_claim_lock();
    g_scope_state = SCOPE_CORE1_CMD_IDLE;
    hw_claim_unlock(save);
    while (1) {
        __wfe();
    }  
}

void scope_core1_poll(void)
{
    uint32_t save;

    if (g_scope_state == SCOPE_CORE1_CMD_FORCE_STOP) {
        // log_printf("force stop done\n");    
        save = hw_claim_lock();
        g_scope_state = SCOPE_CORE1_CMD_IDLE;
        hw_claim_unlock(save);
        while (1) {
            __wfe();
        }   
    }
}

void __no_inline_not_in_flash_func(scope_core1_wait_finish)(void)
{
    while (g_scope_state != SCOPE_CORE1_CMD_IDLE) {}
}